#pragma once


#include "behaviortree_cpp_v3/action_node.h"
#include "rclcpp/rclcpp.hpp"
#include "hnurm_interfaces/msg/vision_recv_data.hpp"
#include "hnurm_interfaces/msg/vision_send_data.hpp"
#include "hnurm_interfaces/msg/target.hpp"
#include <hnurm_interfaces/msg/zone_end_point2_d.hpp>
#include <hnurm_interfaces/msg/special_area.hpp>
#include <hnurm_interfaces/msg/area.hpp>
#include <hnurm_interfaces/msg/type.hpp>
#include <hnurm_interfaces/msg/self_color.hpp>
#include "std_msgs/msg/float32.hpp"
#include <std_msgs/msg/bool.hpp>
#include "geometry_msgs/msg/polygon_stamped.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <nav_msgs/msg/path.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <Eigen/Dense>


#include "deque"
#include "mutex"
#include <atomic>
#include <vector>
#include <algorithm> 
#include <unordered_map>
#include <Eigen/Dense>
#include <cmath>
#include <stdexcept>
// #include "hnurm_decision_interfaces/msg/robot_status.hpp"

namespace hnurm_behavior_trees
{

/**
 * @brief The pubRobotStatus behavior is used to switch the controller
 * that will be used by the controller server. It subscribes to a topic "controller_selector"
 * to get the decision about what controller must be used. It is usually used before of
 * the FollowPath. The selected_controller output port is passed to controller_id
 * input port of the FollowPath
 */
class pubRobotStatus : public BT::SyncActionNode
{
public:
  /**
   * @brief A constructor for nav2_behavior_tree::pubRobotStatus
   *
   * @param xml_tag_name Name for the XML tag for this node
   * @param conf  BT node configuration
   */
  pubRobotStatus(
    const std::string & xml_tag_name,
    const BT::NodeConfiguration & conf);
  ~pubRobotStatus() override;
  /**
   * @brief Creates list of BT ports
   * @return BT::PortsList Containing basic ports along with node-specific ports
   */
  static BT::PortsList providedPorts()
  {
    return {
      BT::OutputPort<bool>("cruise_status","status to control entry of cruise branch"),
      BT::OutputPort<bool>("hp_status","status to control entry of add hp branch"),
      BT::OutputPort<bool>("pursuit_status","status to control entry of pursuit branch"),
      BT::OutputPort<bool>("pursuit_mode1","switch pursuit mode"),
      BT::OutputPort<bool>("pursuit_mode2","switch pursuit mode"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("target_goal","output target goal"),
      BT::OutputPort<bool>("is_hp_ok","check if hp is ok"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("cruise_goal","target cruise goal"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("pursuit_goal","target pursui goal"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("supply_goal","supply goal"),
      BT::OutputPort<geometry_msgs::msg::PoseStamped>("goal_pose_override","goal_pose_override to replace goal_pose publish in rviz2"),
      BT::OutputPort<bool>("navigate_to_pose_status","navigate_to_pose_status"),
      BT::OutputPort<std::string>("current_controller","current_controller"),
      BT::OutputPort<bool>("is_in_special_area","is_in_special_area "),
      BT::OutputPort<std::vector<geometry_msgs::msg::PoseStamped>>("goal_poses","goal poses to compute path through"),
      BT::OutputPort<bool>("do_clear","clear costmatp")
    };
    
  }

  struct current_global_pose
  {
    float pose_x;
    float pose_y;
  };


  struct current_rpy {
      float roll;
      float pitch;
      float yaw;
  };

  struct current_referee_status    //待删除，已经替换成recv_data了
  {
    float current_hp;
    float game_progress;
    current_rpy rpy;
  };

  struct Send_data
  {
    bool target_state;
    float target_distance;
  };

  enum class PursuitType {
      Approach,   
      Away    
  };
  std::atomic<PursuitType> pursuit_type = PursuitType::Approach;
  geometry_msgs::msg::PoseStamped target_pose_;


  // 控制权优先级枚举
  enum class ControlAuthority {
      Idle,         // 空闲状态
      MainCamera,   // 主相机（最高优先级）
      BackCamera    // 后视相机（低优先级）
  };
  
  struct referen_yaw_control
  {
    double reference_yaw_;
    bool on_control;
  };

  // 全局控制权状态
  std::atomic<ControlAuthority> current_authority = ControlAuthority::Idle;
  // 全局变量存储基准Yaw角度
  referen_yaw_control back_camera_control = {0.0,false};
  std::atomic<bool> on_control = false;
   

  Send_data send_data{false,0.0};
  current_global_pose current_pose_{ 0.0,0.0};  //curent robot position in map frame
  std::mutex referee_status_mutex;
  bool get_init_yaw_ = false;
  float init_yaw_;
  current_referee_status referee_status_{0.0,0.0,{0.0f,0.0f,0.0f}};
  float back_target_bias = 0.0;

  //sentry recv data
  struct Seria_Data
  {
    hnurm_interfaces::msg::VisionRecvData data;
    bool is_init=false;
  };
  std::mutex recv_data_mutex;
  Seria_Data recv_data;
  bool is_global_init = false;  //initialize from default.yaml,which execute in the initial_timer_callback

  std::string target_id_;

  //tfs
  geometry_msgs::msg::TransformStamped transform_m2bf;  //transform map to base_footprint
  geometry_msgs::msg::PoseStamped supply_goal;


  // goal_pose_override 
  current_global_pose goal_pose{ 0.0,0.0};


  geometry_msgs::msg::PoseStamped goal_;
  bool goal_pose_updated_ = false;
  bool pub_goal_pose_ = false;
  bool goal_pose_reached_ = false;

  double goal_pose_timeout_ = 100.0;    //似乎timeout参数没什么作用，但是先留着
  rclcpp::Time  goal_pose_timeout_start_time_;

  //special areas 
  std::vector<hnurm_interfaces::msg::SpecialArea> spa_s;
  std::vector<std::string> special_areas_names_;

  struct Areas_with_center{
    hnurm_interfaces::msg::SpecialArea spa;
    current_global_pose center;
    std::vector<hnurm_interfaces::msg::ZoneEndPoint2D> spa_paths_ends_;
  };//areas_with_center;

  std::unordered_map<std::string, Areas_with_center> special_areas_;
  std::string closest_area_;
  bool is_in_special_area = false;

  //cmd_vel remapper
  geometry_msgs::msg::Twist vel_pub_;
  bool cmd_excuting_ =false;
  int direction_selector_ = 0;
  Eigen::Vector2d target_;

  double cmd_yaw_;
  double cmd_yaw_2_;
  struct PID {
  double Kp = 0.5;  // 比例系数
  double Ki = 0.0; // 积分系数
  double Kd = 0.0;  // 微分系数
  double integral = 0.0;
  double prev_error = 0.0;
  rclcpp::Time prev_time;
  };
  PID yaw_pid_;
  PID yaw_pid_inverse_;
  double theta_target;

  //align to the MPPI
  bool first_callback = true;
  rclcpp::Time last_time_;



private:
  /**
   * @brief Function to perform some user-defined operation on tick
   */
  BT::NodeStatus tick() override;

  /**
   * @brief callback function for the controller_selector topic
   *
   * @param msg the message with the id of the controller_selector
   */
  void recv_callback(const hnurm_interfaces::msg::VisionRecvData::SharedPtr msg);
  void back_target_callback(const std_msgs::msg::Float32::SharedPtr msg);
  void send_callback(const hnurm_interfaces::msg::VisionSendData::SharedPtr msg);
  void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
  void target_callback(const hnurm_interfaces::msg::Target::SharedPtr msg);
  void goal_pose_callback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);

  void global_pose_callback(const geometry_msgs::msg::PolygonStamped::SharedPtr msg);
  void initializaion();
  void timer_callback();
  void tf_listener_callback();
  

  void all_off();
  bool goal_checker(current_global_pose cruise_goal,current_global_pose current_pose,double goal_tolerance);
  void goal_updater(current_global_pose goal);
  void remap_cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);

  void path_callback(const nav_msgs::msg::Path::SharedPtr msg);

  //spa
  hnurm_interfaces::msg::SpecialArea getAreas(const std::string &area_name_);
  std::vector<hnurm_interfaces::msg::ZoneEndPoint2D> getPathEnds(const std::string &area_name_);
  current_global_pose getCenter(const std::vector<hnurm_interfaces::msg::ZoneEndPoint2D> &end_points);
  double calculate_distance(current_global_pose current_pose,current_global_pose target_pose);
  std::string getClosestArea();
  void visualizeClosestArea();
  bool isinSpecialArea(current_global_pose current_pose,std::string area);

  void update_poses();  //update path though poses
  rclcpp::Time  path_through_end_time_;  
  double reset_duration_ = 2.0;   //timeout to reset path through task





  std::mutex goal_poses_mutex;
  std::vector<geometry_msgs::msg::PoseStamped> goal_poses_;
  bool is_executing_ = false;
  std::mutex diff_yaw_mutex;
  double yaw_diff;
  double calculateAngle(const Eigen::Vector2d& v1, const Eigen::Vector2d& v2);

  
  geometry_msgs::msg::PoseStamped update_goal(current_global_pose cruise_goal); 
  current_global_pose find_Away_point(float yaw,float distance);


  //cruise 
  struct current_cruise_goal
  {
    int idx;
    bool former_is_reached;
  };

  current_cruise_goal cruise_iterator{0,false};   //cruise goal iterator
  bool is_waiting_ = false;
  double wait_duration_ = 2.0;
  rclcpp::Time wait_start_time_;
  double timeout_ = 10.0;
  rclcpp::Time timeout_start_time_;

  current_global_pose getClosestBuff();
  

  //subscribers
  rclcpp::Subscription<hnurm_interfaces::msg::VisionRecvData>::SharedPtr recv_sub_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr back_target_sub_;
  rclcpp::Subscription<hnurm_interfaces::msg::VisionSendData>::SharedPtr send_sub_;

  rclcpp::Subscription<hnurm_interfaces::msg::Target>::SharedPtr target_sub_;
  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;

  rclcpp::Subscription<geometry_msgs::msg::PolygonStamped>::SharedPtr global_pose_sub_;
  
  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr goal_pose_override_sub_;
  rclcpp::Subscription<hnurm_interfaces::msg::SpecialArea>::SharedPtr special_areas_sub_;

  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
  rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;

  
 //publihser
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pursuit_pub_;
  rclcpp::Publisher<hnurm_interfaces::msg::VisionSendData>::SharedPtr decision_pub_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray >::SharedPtr marker_pub_;
  rclcpp::Publisher<hnurm_interfaces::msg::SpecialArea>::SharedPtr special_areas_pub_;
  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr in_special_area_pub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_remap_pub_;





  rclcpp::Node::SharedPtr node_;
  rclcpp::CallbackGroup::SharedPtr callback_group_;
  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;
  rclcpp::CallbackGroup::SharedPtr callback_group_for_vision_data;
  rclcpp::executors::SingleThreadedExecutor callback_group_executor_for_vision_data;

  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::TimerBase::SharedPtr tf_listener_timer_;

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  // declare subscriptions
  std::string recv_topic_;
  std::string back_target_;
  std::string send_topic_;
  std::string target_topic_;
  std::string global_position_;
  std::string pursuit_armor_id_;
  std::string decision_send_topic_;
  std::string odom_topic_;
  float hp_threshold;
  std::string goal_pose_override_;
  std::string area_pub_topic_;

  int decision_selector_;

  //command velocity remapper
  std::string cmd_vel_remap_topic_;
  std::string plan_path_topic_;

  std::vector<current_global_pose> cruise_goals_;
  std::vector<current_global_pose> buff_points_;
  current_global_pose enermy_outpost_;
  current_global_pose attack_outpost_point_;

  //FSM global status
  bool pursuit_ = false;
  bool cruise_ =false;
  bool supply_ =false;
  bool attack_outpost_ = false;
  bool stay_ = false;


  

  std::mutex transform_mutex;
  std::thread executor_thread_;

  std::mutex mtx_;
  std::thread wait_thread_;

  std::deque<float> bias_window_;  // slide window
  const int WINDOW_SIZE = 10;       
};
}  // namespace hnurm_behavior_trees

